//
//  PMPivotJoint.m
//  PezMonkey
//
//  Created by Spence DiNicolantonio on 10/31/09.
//  Copyright 2009 Necrosoft. All rights reserved.
//

/**
 * A subclass of PMConstraint for representing pivot joints. A PMPivotJoint Connects 
 * two PMBodys at a single point, allowing only rotational motion. This type of 
 * joint can be thought of like placing a single pin through a piece of paper, 
 * onto the wall.
 */

#import "PMPivotJoint.h"
#import "PMBody.h"


@implementation PMPivotJoint


#pragma mark -
#pragma mark Init

/**
 * Returns a PMPivotJoint object initialized by anchoring two given rigid bodies 
 * at their center of gravity.
 */
- (id)initWithBodyA:(PMBody *)aBody bodyB:(PMBody *)otherBody {
	return [self initWithAnchor:PMVectZero 
						onBodyA:aBody 
						 anchor:PMVectZero 
						onBodyB:otherBody];
}

/**
 * Returns a PMPivotJoint object initialized by anchoring two given rigid bodies 
 * at given positions relative to the corresponding body's center of gravity.
 */
- (id)initWithAnchor:(PMVect)anchorA onBodyA:(PMBody *)aBody anchor:(PMVect)anchorB onBodyB:(PMBody *)otherBody {
	if (![super init])
		return nil;
	
	// allocate and initialize cpConstraint
	constraint = cpPivotJointNew2(aBody.cpBody, otherBody.cpBody, anchorA, anchorB);
	
	// set body references
	self.bodyA = aBody;
	self.bodyB = otherBody;
	
	return self;
}

/**
 * Returns a PMPivotJoint object initialized by anchoring two given rigid bodies at 
 * a given pivot point. The pivot point should be given in world coordinates.
 */
- (id)initWithBodyA:(PMBody *)aBody bodyB:(PMBody *)otherBody pivot:(PMVect)pivot {
	if (![super init])
		return nil;
	
	// allocate and initialize cpConstraint
	constraint = cpPivotJointNew(aBody.cpBody, otherBody.cpBody, pivot);
	
	// set body references
	self.bodyA = aBody;
	self.bodyB = otherBody;
	
	// reference this PMConstraint object with cpConstraint's data pointer
	// this is for internal use
	constraint->data = self;
	
	return self;
}


#pragma mark Convenience

/**
 * Creates and returns a PMPivotJoint object initialized by anchoring two given 
 * rigid bodies at their center of gravity.
 */
+ (PMPivotJoint *)pivotJointWithBodyA:(PMBody *)aBody bodyB:(PMBody *)otherBody {
	return [[[PMPivotJoint alloc] initWithBodyA:aBody 
										  bodyB:otherBody] autorelease];
}

/**
 * Creates and returns a PMPivotJoint object initialized by anchoring two given 
 * rigid bodies at given positions relative to the corresponding body's center 
 * of gravity.
 */
+ (PMPivotJoint *)pivotJointWithAnchor:(PMAnchor)anchorA onBodyA:(PMBody *)aBody anchor:(PMAnchor)anchorB onBodyB:(PMBody *)otherBody {
	return [[[PMPivotJoint alloc] initWithAnchor:anchorA 
										 onBodyA:aBody 
										  anchor:anchorB 
										 onBodyB:otherBody] autorelease];
}

/**
 * Creates and returns a PMPivotJoint object initialized by anchoring two given 
 * rigid bodies at a given pivot point. The pivot point should be given in world 
 * coordinates.
 */
+ (PMPivotJoint *)pivotJointWithBodyA:(PMBody *)aBody bodyB:(PMBody *)otherBody pivot:(PMVect)pivot {
	return [[[PMPivotJoint alloc] initWithBodyA:aBody 
										  bodyB:otherBody 
										  pivot:pivot] autorelease];
}


#pragma mark -
#pragma mark Getters

/**
 * Returns the receiver's joint anchor position for bodyA.
 */
- (PMAnchor)anchorA {
	return cpPivotJointGetAnchr1(constraint);
}

/**
 * Returns the receiver's joint anchor position for bodyB.
 */
- (PMAnchor)anchorB {
	return cpPivotJointGetAnchr2(constraint);
}

/**
 * Returns the receiver's joint anchor position for the body corresponding to the 
 * given key. Returns PMVectZero if there is no body/anchor for the given key.
 */
- (PMAnchor)anchorForBodyWithKey:(PMConstraintBodyKey)bodyKey {
	switch (bodyKey) {
		case PMConstraintBodyKeyA:
			return [self anchorA];
		case PMConstraintBodyKeyB:
			return [self anchorB];
		default:
			return PMVectZero;
	}
}

/**
 * Returns the receiver's pivot point in world coordinates.
 */
- (PMVect)pivot {
	return [self.bodyA convertPointFromLocal:[self anchorA]];
}


#pragma mark -
#pragma mark Setters

/**
 * Sets the joint anchor position of the receiver's bodyA to a given point relative
 * that body's center of gravity.
 */
- (void)setAnchorA:(PMAnchor)anchor {
	cpPivotJointSetAnchr1(constraint, anchor);
}

/**
 * Sets the joint anchor position of the receiver's bodyB to a given point relative
 * that body's center of gravity.
 */
- (void)setAnchorB:(PMAnchor)anchor {
	cpPivotJointSetAnchr2(constraint, anchor);
}

/**
 * Sets the joint anchor position of the receiver's body corresponding to the given
 * key, to a given point relative that body's center of gravity.
 */
- (void)setAnchor:(PMAnchor)anchor forBodyWithKey:(PMConstraintBodyKey)bodyKey {
	switch (bodyKey) {
		case PMConstraintBodyKeyA:
			return [self setAnchorA:anchor];
		case PMConstraintBodyKeyB:
			return [self setAnchorB:anchor];
	}
}

/**
 * Sets the receiver's pivot to a given point in world coordinates.
 */
- (void)setPivot:(PMVect)pivot {
	((cpPivotJoint *) constraint)->anchr1 = cpvunrotate(cpvsub(pivot, self.bodyA.cpBody->p), 
														self.bodyA.cpBody->rot);
	((cpPivotJoint *) constraint)->anchr2 = cpvunrotate(cpvsub(pivot, self.bodyB.cpBody->p), 
														self.bodyB.cpBody->rot);
}

@end
